#include "AI_Agent.h"
#include "Shapes/OgreBulletCollisionsBoxShape.h"
#include "Shapes/OgreBulletCollisionsCompoundShape.h"
#include "OgreBulletDynamicsRigidBody.h"

using namespace Ogre;
using namespace OgreBulletCollisions;
using namespace OgreBulletDynamics;

static float	gKartMass = 400.0f;
static float	gMaxEngineForce = 2650.0f;
static float	gMaxBrakeForce = -2650.0f;

static float	gSteeringIncrement = 0.05f;
static float	gSteeringClamp = 0.4f;
static float	gSteeringWheelLock = 90.0f;

static float	gFrontWheelRadius = 0.5f;
static float	gRearWheelRadius = 0.6075f;
static float	gWheelWidth = 0.3645f;

static float	gFrontWheelFriction = 20.0f;
static float	gBackWheelFriction = 20.0f;
static float	gSuspensionStiffness = 200.0f;
static float	gSuspensionDamping = 2.3f;
static float	gSuspensionCompression = 4.4f;
static float	gMaxSuspensionForce = 6000.0f;

static float	gRollInfluence = 0.05f;
static float	gSuspensionRestLength = 0.4f;
static float	gMaxSuspensionTravelCm = 300.0f;
static float	gFrictionSlip = 10.5f;

AI_Agent::AI_Agent(int id, AI_TrackPath *trackPath, Ogre::SceneManager *SceneMgr, OgreBulletDynamics::DynamicsWorld *World, Ogre::Vector3 kartPosition,
	               std::vector<TimingGate> *gates, Real *raceTime)
{
	mSceneMgr = SceneMgr;
	mWorld = World;
	mID = id;
	currWaypointIndex = 0;
	mTrackPath = trackPath;
	mStuckTimer = 5000;

	mTimingGates = gates;
	mRaceTime = raceTime;
	mNextGate = 1;
	mCurrLap = 1;

	for(int i = 0; i < mTimingGates->size(); i++)
	{
		mLastTimeAtTimingGate.push_back(INT_MAX);
	}
	
	for (int i = 0; i < 4; i++)
    {
        mWheelsEngine[i] = 0;
        mWheelsSteerable[i] = 0;
    }
    mWheelsEngineCount = 4;
    mWheelsEngine[0] = 0;
    mWheelsEngine[1] = 1;
    mWheelsEngine[2] = 2;
    mWheelsEngine[3] = 3;

    mWheelsSteerableCount = 2;
    mWheelsSteerable[0] = 0;
    mWheelsSteerable[1] = 1;

    mWheelEngineStyle = 0;
    mWheelSteeringStyle = 0;

    mSteeringLeft = false;
    mSteeringRight = false;

    mEngineForce = 0;
	mBrakeForce = 0;
    mSteering = 0;
	mSteeringTarget = 0;

	createKart(kartPosition);
}

AI_Agent::~AI_Agent()
{
}

bool AI_Agent::createKart(Ogre::Vector3 kartPosition)
{
	try
	{
		const Ogre::Vector3 SteeringWheelShift(0.0f, 0.9375f, 0.925f);

		mChassis = mSceneMgr->createEntity("chassis" + StringConverter::toString(mID), "Kart_Chassis.mesh");
		mSteeringWheel = mSceneMgr->createEntity("Kart_Steering_Wheel" + StringConverter::toString(mID), "Kart_Steering_Wheel.mesh");

		SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();

		mChassisNode = node->createChildSceneNode();
		mChassisNode->attachObject(mChassis);

		SceneNode *node2 = node->createChildSceneNode();
		mSteeringWheelNode = node2->createChildSceneNode();
		mSteeringWheelNode->attachObject(mSteeringWheel);
		mSteeringWheelNode->pitch(Degree(-60));
		mSteeringWheelNode->setPosition(SteeringWheelShift);

		mChassis->setQueryFlags(GEOMETRY_QUERY_MASK);
		mSteeringWheel->setQueryFlags(GEOMETRY_QUERY_MASK);
	#if (OGRE_VERSION < ((1 << 16) | (5 << 8) | 0)) // only applicable before shoggoth (1.5.0)
		mChassis->setNormaliseNormals(true);

	#endif
		mChassis->setCastShadows(true);
		mSteeringWheel->setCastShadows(true);

		BoxCollisionShape* chassisShape = new BoxCollisionShape(Ogre::Vector3(2.625f,0.5f,4.29375f));
		CompoundCollisionShape* compound = new CompoundCollisionShape();
		compound->addChildShape(chassisShape, Vector3::ZERO);

		mCarChassis = new WheeledRigidBody("Kart_Chassis" + StringConverter::toString(mID), mWorld);

		mCarChassis->setShape(node, compound, 0.6f, 0.6f, 0.3f, gKartMass, kartPosition, Quaternion::IDENTITY);
		mCarChassis->setDamping(0.2f, 0.2f);
		mCarChassis->disableDeactivation();

		mTuning = new VehicleTuning(
			gSuspensionStiffness,
			gSuspensionCompression,
			gSuspensionDamping,
			gMaxSuspensionTravelCm,
			gFrictionSlip);

		mVehicleRayCaster = new VehicleRayCaster(mWorld);
		mVehicle = new RaycastVehicle(mCarChassis, mTuning, mVehicleRayCaster);
		{
			int rightIndex = 0;
			int upIndex = 1;
			int forwardIndex = 2;

			mVehicle->setCoordinateSystem(rightIndex, upIndex, forwardIndex);

			Ogre::Vector3 wheelDirectionCS0(0,-1,0);
			Ogre::Vector3 wheelAxleCS(-1,0,0);

			mWheels[0] = mSceneMgr->createEntity("Kart_Tire_FL" + StringConverter::toString(mID), "Kart_Tire_FL.mesh");
			mWheels[1] = mSceneMgr->createEntity("Kart_Tire_FR" + StringConverter::toString(mID), "Kart_Tire_FR.mesh");
			mWheels[2] = mSceneMgr->createEntity("Kart_Tire_RL" + StringConverter::toString(mID), "Kart_Tire_RL.mesh");
			mWheels[3] = mSceneMgr->createEntity("Kart_Tire_RR" + StringConverter::toString(mID), "Kart_Tire_RR.mesh");
			mWheels[0]->setQueryFlags(GEOMETRY_QUERY_MASK);
			mWheels[1]->setQueryFlags(GEOMETRY_QUERY_MASK);
			mWheels[2]->setQueryFlags(GEOMETRY_QUERY_MASK);
			mWheels[3]->setQueryFlags(GEOMETRY_QUERY_MASK);
	#if (OGRE_VERSION < ((1 << 16) | (5 << 8) | 0)) // only applicable before shoggoth (1.5.0)
			mWheels[0]->setNormaliseNormals(true);
			mWheels[1]->setNormaliseNormals(true);
			mWheels[2]->setNormaliseNormals(true);
			mWheels[3]->setNormaliseNormals(true);
	#endif
			mWheels[0]->setCastShadows(true);
			mWheels[1]->setCastShadows(true);
			mWheels[2]->setCastShadows(true);
			mWheels[3]->setCastShadows(true);

			mWheelNodes[0] = mSceneMgr->getRootSceneNode()->createChildSceneNode();
			mWheelNodes[1] = mSceneMgr->getRootSceneNode()->createChildSceneNode();
			mWheelNodes[2] = mSceneMgr->getRootSceneNode()->createChildSceneNode();
			mWheelNodes[3] = mSceneMgr->getRootSceneNode()->createChildSceneNode();
			mWheelNodes[0]->attachObject(mWheels[0]);
			mWheelNodes[1]->attachObject(mWheels[1]);
			mWheelNodes[2]->attachObject(mWheels[2]);
			mWheelNodes[3]->attachObject(mWheels[3]);
			{
				bool isFrontWheel = true;

				Ogre::Vector3 connectionPointCS0(1.875f, 0.075f, 2.65625f);

				mVehicle->addWheel(
					mWheelNodes[0],
					connectionPointCS0,
					wheelDirectionCS0,
					wheelAxleCS,
					gSuspensionRestLength,
					gFrontWheelRadius,
					isFrontWheel, gFrontWheelFriction, gRollInfluence, gMaxSuspensionForce);

				connectionPointCS0 = Ogre::Vector3(-1.875f, 0.075f, 2.65625f);

				mVehicle->addWheel(
					mWheelNodes[1],
					connectionPointCS0,
					wheelDirectionCS0,
					wheelAxleCS,
					gSuspensionRestLength,
					gFrontWheelRadius,
					isFrontWheel, gFrontWheelFriction, gRollInfluence, gMaxSuspensionForce);

				connectionPointCS0 = Ogre::Vector3(1.875f, 0.25f, -3.23125f);

				isFrontWheel = false;
				mVehicle->addWheel(
					mWheelNodes[2],
					connectionPointCS0,
					wheelDirectionCS0,
					wheelAxleCS,
					gSuspensionRestLength,
					gRearWheelRadius,
					isFrontWheel, gBackWheelFriction, gRollInfluence, gMaxSuspensionForce);

				connectionPointCS0 = Ogre::Vector3(-1.875f, 0.25f, -3.23125f);

				mVehicle->addWheel(
					mWheelNodes[3],
					connectionPointCS0,
					wheelDirectionCS0,
					wheelAxleCS,
					gSuspensionRestLength,
					gRearWheelRadius,
					isFrontWheel, gBackWheelFriction, gRollInfluence, gMaxSuspensionForce);
			}
		}
		mEngineNode = mChassisNode->createChildSceneNode("Kart_Engine" + StringConverter::toString(mID));
		Vector3 pos = (mWheelNodes[2]->_getDerivedPosition() + mWheelNodes[3]->_getDerivedPosition()) / 2;
		pos.y += 0.25f;
		mEngineNode->setPosition(pos);
		mEngineNode->setOrientation(mChassisNode->getOrientation());
		mPrevLocation = mChassisNode->_getDerivedPosition();
	} catch(const std::ios_base::failure& e)
	{
		return false;
	}
	return true;
}

void AI_Agent::incNextGate()
{
	mLastTimeAtTimingGate[mNextGate] = *mRaceTime;
	if(mNextGate == mTimingGates->size()-1)
	{
		mNextGate = 0;
	}
	else
	{
		++mNextGate;
		if (mNextGate == 1)
		{
			++mCurrLap;
		}
	}
}

float AI_Agent::DistanceToNextTimingGate()
{
	return (mChassisNode->_getDerivedPosition() - mTimingGates->at(mNextGate).center).length();
}

void AI_Agent::updateGateTimers()
{
	if(DistanceToNextTimingGate() <= (mTimingGates->at(mNextGate).radius + 2.9f))
	{
		incNextGate();
	}
}

bool AI_Agent::checkIfStuck()
{
	if((mPrevLocation - mChassisNode->_getDerivedPosition()).squaredLength() < 30)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void AI_Agent::applyUnstuckImpulse()
{
	Vector3 offset = mWheelNodes[2]->_getDerivedPosition() - mWheelNodes[0]->_getDerivedPosition();
	mCarChassis->applyForce(Vector3(0,150000, 0), offset);
}

bool AI_Agent::update(Ogre::Real elapsedTime, GameState state)
{
	mGameState = state;
	if(mGameState == RACING)
	{
		mStuckTimer -= elapsedTime/1000;
		if(mStuckTimer < 0)
		{
			mStuckTimer = 5000;
			if(checkIfStuck())
			{
				applyUnstuckImpulse();
			}
			mPrevLocation = mChassisNode->_getDerivedPosition();
		}
		setSteeringToNextWaypoint();
		setThrottleToNextWaypoint();

		if (mSteeringLeft)
		{
			if (mSteering + gSteeringIncrement > (gSteeringClamp * mSteeringTarget))
			{
				float deltaSteer = (gSteeringClamp * mSteeringTarget) - mSteering;
				mSteering = (gSteeringClamp * mSteeringTarget);
				mSteeringWheelNode->yaw(Degree(deltaSteer/gSteeringClamp*(gSteeringWheelLock/2.0f)));
			} else
			{
				mSteering += gSteeringIncrement;
				mSteeringWheelNode->yaw(Degree(gSteeringIncrement/gSteeringClamp*(gSteeringWheelLock/2.0f)));
			}
		}
		else if (mSteeringRight)
		{
			if (mSteering - gSteeringIncrement < (gSteeringClamp * mSteeringTarget))
			{
				float deltaSteer = mSteering - (gSteeringClamp * mSteeringTarget);
				mSteering = (gSteeringClamp * mSteeringTarget);
				mSteeringWheelNode->yaw(Degree(-deltaSteer/gSteeringClamp*(gSteeringWheelLock/2.0f)));
			} else
			{
				mSteering -= gSteeringIncrement;
				mSteeringWheelNode->yaw(Degree(-gSteeringIncrement/gSteeringClamp*(gSteeringWheelLock/2.0f)));
			}
		}
		else if (mSteering > 0.01)
		{
			mSteering -= gSteeringIncrement * 0.5f;
			mSteeringWheelNode->yaw(Degree((-gSteeringIncrement * 0.5f)/gSteeringClamp*(gSteeringWheelLock/2.0f)));
		}
		else if (mSteering < -0.01)
		{
			mSteering += gSteeringIncrement * 0.5f;
			mSteeringWheelNode->yaw(Degree((gSteeringIncrement * 0.5f)/gSteeringClamp*(gSteeringWheelLock/2.0f)));
		}

		// apply Steering on relevant wheels
		for (int i = mWheelsSteerable[0]; i < mWheelsSteerableCount; i++)
		{
			if (i < 2)
				mVehicle->setSteeringValue(mSteering, mWheelsSteerable[i]);
			else
				mVehicle->setSteeringValue(-mSteering, mWheelsSteerable[i]);
		}

		// apply engine Force on relevant wheels
		for (int i = mWheelsEngine[0]; i < mWheelsEngineCount; i++)
		{
			mVehicle->applyEngineForce(mEngineForce + mBrakeForce, mWheelsEngine[i]);
		}
		updateGateTimers();
	}
	
    return true;
}

//Steers the kart left or right. Float value between -1 and 1. -1 = full lock left, 1 = full lock right.
void AI_Agent::steerKart(float dir)
{
	if (dir > 1)
		dir = 1;
	if (dir < -1)
		dir = -1;

	if (dir < -0.025)
	{
		mSteeringLeft = true;
		mSteeringRight = false;
		mSteeringTarget = dir;
	} else if (dir > 0.025)
	{
		mSteeringLeft = false;
		mSteeringRight = true;
		mSteeringTarget = dir;
	} else
	{
		mSteeringLeft = false;
		mSteeringRight = false;
		mSteeringTarget = 0;
	}
}

//Causes the kart to accelerate. Float value between -1 and 1. -1 = full breaking force, 1 = full engine force.
void AI_Agent::accelerateOrBrake(float dir)
{
	if (dir > 1)
		dir = 1;
	if (dir < -1)
		dir = -1;
	mEngineForce = gMaxEngineForce * dir;
}

//returns a normalized forward vector of the kart with zero y axis.
Ogre::Vector3 AI_Agent::getForwardVector()
{
	/*Matrix3 matrix;
	mChassisNode->getOrientation().ToRotationMatrix(matrix);
	return matrix.GetColumn(2);*/
	Vector3 forwardVector = mWheelNodes[0]->_getDerivedPosition() - mWheelNodes[2]->_getDerivedPosition();
	forwardVector.y = 0;
	forwardVector.normalise();	
	return forwardVector;
}

Ogre::Real AI_Agent::getVelocity()
{
	return mCarChassis->getLinearVelocity().length();
}

void AI_Agent::attachSoundtoEngine(String soundName, OgreOggSound::OgreOggSoundManager *soundMngr)
{
	mEngineNode->attachObject(soundMngr->getSound(soundName));
}

int AI_Agent::getID()
{
	return mID;
}

void AI_Agent::incWaypointIndex()
{
	if(currWaypointIndex >= mTrackPath->mWaypoints->size()-1)
	{
		currWaypointIndex = 0;
	}
	else
	{
		++currWaypointIndex;
	}
}

void AI_Agent::setSteeringToNextWaypoint()
{
	if(checkDistanceToNextWaypoint() < mTrackPath->mWaypoints->at(currWaypointIndex).mMaxRadius)
	{
		incWaypointIndex();
	}
	steerKart(calculateSteeting()/(gSteeringWheelLock/2.0f));
}

void AI_Agent::setThrottleToNextWaypoint()
{
	accelerateOrBrake(mTrackPath->mWaypoints->at(currWaypointIndex).mMaxSpeed / getVelocity());
}

Real AI_Agent::checkDistanceToNextWaypoint()
{
	Vector3 pointAhead = getForwardVector();
	pointAhead = pointAhead * 3.0f; //This vector now points 5 units in the direction ahead of the kart.
	pointAhead = pointAhead + mChassisNode->_getDerivedPosition(); //move the vector to the kart's current position
	pointAhead.y = 0;
	return (pointAhead - mTrackPath->mWaypoints->at(currWaypointIndex).mPosition).length();
}

//Calculates the steering angle needed to head towards the next waypoint.
float AI_Agent::calculateSteeting()
{
	Vector3 pointAhead = getForwardVector();
	pointAhead = pointAhead * 3.0f; //This vector now points just in front of the kart (basically the front bumper).
	pointAhead = pointAhead + mChassisNode->_getDerivedPosition(); //move the vector to the kart's current position
	Vector3 pointToWaypoint = mTrackPath->mWaypoints->at(currWaypointIndex).mPosition - pointAhead;
	pointToWaypoint.y = 0;
	pointToWaypoint.normalise();
	Vector3 kartDir = getForwardVector();

	float sina = kartDir.crossProduct(pointToWaypoint).length();
	float cosa = kartDir.dotProduct(pointToWaypoint);
	float angleInDegrees = Radian(atan2(sina, cosa)).valueDegrees();
	Vector3 yAxis = Vector3(0,1,0);
	float sign = yAxis.dotProduct(kartDir.crossProduct(pointToWaypoint));
	if(sign<0)
	{
		angleInDegrees = -angleInDegrees;
	}

	return angleInDegrees;
}

int AI_Agent::getCurrentLap()
{
	return mCurrLap;
}

int AI_Agent::getLastTimingGate()
{
	if(mNextGate == 0)
	{
		return mTimingGates->size()-1;
	}
	else
	{
		return mNextGate - 1;
	}
}

Real AI_Agent::getLastTimingGateTime()
{
	return mLastTimeAtTimingGate[getLastTimingGate()];
}